#include <Servo.h>  // we need this library because we are using a servo
#include <Protobot.h>  // the Protobot library contains the functions made specifically for this minibot.

Protobot robot;  // create a Protobot object se we can use its functions

int sweepAngle=20;
int targetAngle;
int targetDistance=30;
int followDistance = 40;
bool searchFailed = false;

void setup()
{
  // put your setup code here, to run once:
  robot.begin();  // sets up the hardware connections
  Serial.begin(9600);  // start USB serial interface 
  delay(3000); // wait 3 seconds
  Serial.println("hello, world!"); // say hello to anyone who is listening
}
int findTargetAngle()
{
  robot.turnServo(20);
  delay(100);
  while (sweepAngle < 160)
  {
    sweepAngle = sweepAngle +10;
    robot.turnServo(sweepAngle);
    delay(25);
    if (robot.readSensor() < followDistance)
    {
      targetAngle = sweepAngle;
      return targetAngle;
      sweepAngle = 160;
    }
  }
  sweepAngle = 20;
  robot.turnServo(sweepAngle);
}
int findTargetDistance()
{
  targetDistance = robot.readSensor();
  return targetDistance;
  String distanceMessage = "Distance From Target is: " + robot.readSensor();
  Serial.println(distanceMessage);
}
void botRotate(int targetAnge)
{
  int timeRotate;
  if (targetAngle > 90)
  {
    timeRotate = targetAngle - 90;
    timeRotate = timeRotate*32.02778;
    robot.turnRight(128);
    delay(timeRotate);
    robot.stop();
    delay(100);
  }
  else
  {
    timeRotate = 90 - targetAngle;
    timeRotate = timeRotate * 32.02778;
    robot.turnLeft(128);
    delay(timeRotate);
    robot.stop();
    delay(100);
  }
  String angleRotated = "The bot rotated";
  Serial.println(angleRotated);
  delay(100);
}
void loop()
{
  while (robot.readSensor() > followDistance)
  {
    for (int n=3; n>0; n--)
    {
      findTargetAngle();
      delay(500);
    }
    if (robot.readSensor() > followDistance)
    {
      robot.turnLeft(128);
      delay(90*32.02778);
      robot.stop();
    }
  }
  Serial.println(targetAngle);
  botRotate(targetAngle);
  robot.turnServo(90);
  targetAngle = 90;
  while(robot.readSensor() > 30)
  {
    robot.forward(255);
  }
  robot.stop();
}
